package vs.tech.auto.imu;

import java.util.ArrayList;

import android.content.Context;
import android.hardware.Sensor;
import android.hardware.SensorEvent;
import android.hardware.SensorEventListener;
import android.hardware.SensorManager;
import android.os.SystemClock;
import android.widget.TextView;

public class IMU extends Thread implements SensorEventListener
{
	///	Local Information about how the thread was created.
	private Context loc_Context;
	private DisplayHandles mParentDisplayHandles = new DisplayHandles();
		
	///	Filtering variables
	private int LengthOfFiltering_RTI = 10;
	
	private ArrayList ml_accel_X = new ArrayList();
	private ArrayList ml_accel_Y = new ArrayList();
	private ArrayList ml_accel_Z = new ArrayList();
	private ArrayList timestep = new ArrayList();
	
	//	Timekeeping
	private long lastTime_msec = 0;
	private long currTime_msec = 0;
	private int msec = 0;
	private float averageExec = 0;
	
	///	Display vars
	private float accel_X = 0;
	private float accel_Y = 0;
	private float accel_Z = 0;
	private float RAW_accel_X = 0;
	private float RAW_accel_Y = 0;
	private float RAW_accel_Z = 0;
	private float vel_X = 0;
	private float vel_Y = 0;
	private float vel_Z = 0;
	private float dpos_X = 0;
	private float dpos_Y = 0;
	private float dpos_Z = 0;
	private float mPitch = 0;
	private float mRoll = 0;
	private float mYaw = 0;
	
	//	Orientation vars
	private float[] R = new float[9];
	private float[] I = new float[9];
	private float[] gravity = new float[3];
	private float[] geomag = new float[3];
	private float[] orientation = new float[3];
	
	public IMU(Context argContext, DisplayHandles argDisp)
	{
		super();
		loc_Context = argContext;
		
		//	Get the textviews
		mParentDisplayHandles = argDisp;
		

	}

	@Override
	public void onAccuracyChanged(Sensor sensor, int accuracy) 
	{

		
	}

	@Override
	public void onSensorChanged(SensorEvent event)
	{
		synchronized (this){
			
			if ( lastTime_msec == 0 ) lastTime_msec = SystemClock.elapsedRealtime();
			currTime_msec = SystemClock.elapsedRealtime();
			msec = (int)(currTime_msec - lastTime_msec);	
			lastTime_msec = currTime_msec;
			
		if ( event.sensor.getType() == Sensor.TYPE_ACCELEROMETER )
		{
			RAW_accel_X = event.values[0];
			RAW_accel_Y = event.values[1];
			RAW_accel_Z = event.values[2];
		}
		if ( event.sensor.getType() == Sensor.TYPE_MAGNETIC_FIELD )
		{
			//	Place values into gravity
			gravity[0] = RAW_accel_X;
			gravity[1] = RAW_accel_Y;
			gravity[2] = RAW_accel_Z;
			
			//	Get magnetic field
			geomag = event.values;
			
			if ( SensorManager.getRotationMatrix(R, I, gravity, geomag))
			{
				SensorManager.getOrientation(R, orientation);
				mYaw = (float)Math.toDegrees(orientation[0]);
				mPitch = (float)Math.toDegrees(orientation[1]);
				mRoll = (float)Math.toDegrees(orientation[2]);
				
				
			}
		}		
		
		Update();		
		
		}
	}
	
	public void Update()
	{
		UpdateFiltering();
		
		mParentDisplayHandles.accel_X.setText("Accel X = " + Float.toString(accel_X));
		mParentDisplayHandles.accel_Y.setText("Accel Y = " + Float.toString(accel_Y));
		mParentDisplayHandles.accel_Z.setText("Accel Z = " + Float.toString(accel_Z));
		mParentDisplayHandles.vel_X.setText("Vel X = " + Float.toString(vel_X));
		mParentDisplayHandles.vel_Y.setText("Vel Y = " + Float.toString(vel_Y));
		mParentDisplayHandles.vel_Z.setText("Vel Z = " + Float.toString(vel_Z));
		mParentDisplayHandles.dpos_X.setText("DeltaPos X = " + Float.toString(dpos_X));
		mParentDisplayHandles.dpos_Y.setText("DeltaPos Y = " + Float.toString(dpos_Y));
		mParentDisplayHandles.dpos_Z.setText("DeltaPos Z = " + Float.toString(dpos_Z));
		mParentDisplayHandles.pitch.setText("Pitch (deg) = " + Float.toString(mPitch));
		mParentDisplayHandles.roll.setText("Roll (deg) = " + Float.toString(mRoll));
		mParentDisplayHandles.yaw.setText("Yaw (deg) = " + Float.toString(mYaw));
		mParentDisplayHandles.averageExec.setText("Average Execution Time (msec) = " + Float.toString(averageExec));
		mParentDisplayHandles.buffersize.setText("Size of time Buffer = " + Integer.toString(timestep.size()));
		
		
	}
	
	public void UpdateFiltering()
	{
		ml_accel_X.add(RAW_accel_X);
		ml_accel_Y.add(RAW_accel_Y);
		ml_accel_Z.add(RAW_accel_Z);
		timestep.add((float)msec);
		
		if ( timestep.size() > 500 )	timestep.remove(0);
		averageExec = GetAverage(timestep);
		
		
		if ( ml_accel_X.size() > LengthOfFiltering_RTI )
		{
			ml_accel_X.remove(0);
			ml_accel_Y.remove(0);
			ml_accel_Z.remove(0);
		}
		
		accel_X = GetAverage( ml_accel_X );
		accel_Y = GetAverage( ml_accel_Y );
		accel_Z = GetAverage( ml_accel_Z );
		
		if ( msec != 0 )
		{
			vel_X += Integrate( accel_X );
			vel_Y += Integrate( accel_Y );
			vel_Z += Integrate( accel_Z );
			
			dpos_X += Integrate( vel_X );
			dpos_Y += Integrate( vel_Y );
			dpos_Z += Integrate( vel_Z );
		}
		
	}
	
	private float GetAverage( ArrayList<Float> arg )
	{
		float total = 0;
		
		for ( int i = 0; i < arg.size(); i++ )
		{
			total += arg.get(i);			
		}
		
		return ( total / (float)arg.size() );		
	}
	
	private float Integrate (float arg)
	{
		return (float)(arg * msec * 0.001);
	}
	
	@Override
	public void run()
	{

	}

}
